#!/usr/bin/env python3
# coding:utf-8
"""
两轮差速运动仿真
"""
import sys
import threading
import rclpy
from rclpy.node import Node
# from std_msgs.msg import String, Float64, Float64MultiArray, Int64, UInt64MultiArray
from geometry_msgs.msg import Twist
from nobloack_key_input import Noblock_terminal


class MinimalPublisher(Node):
    def __init__(self):
        super().__init__('sim_diff_drive')
        self.pub_cmd = self.create_publisher(Twist, "/demo/cmd_demo", 1)
        self.thread_init()

    def thread_init(self):
        t1 = threading.Thread(target=self.control_cmd, daemon=True)
        t1.start()

    def control_cmd(self):
        help_str="""
\033[80D    w +0.1 m/s
\033[80D a -0.1 , s +0.1  rad/s
\033[80D    s -0.1 m/s
\033[80D 空格 角度,速度为0
\033[80D h帮助, q 退出
\033[80D
"""
        print(help_str)
        noblock_term = Noblock_terminal()
        count = 0
        speed = 0.0
        angle = 0.0
        msg = Twist()
        while True:
            # key = noblock_term.select_cmd(0.2)
            key = noblock_term.noblock_get_cmd()
            if key == 'w':
                speed += 0.1
            elif key == 's':
                speed -= 0.1
                if speed < 0.0:
                    speed = 0.0
            elif key == 'a':
                angle -= 0.1
            elif key == 'd':
                angle += 0.1
            elif key == ' ':
                speed = 0.0
                angle = 0.0
            elif key == 'h':
                print(help_str)
            elif key == 'q':
                break
            
            msg.linear.x = speed
            msg.angular.z = angle
            self.pub_cmd.publish(msg)
            print(f"\033[80D speed:{speed} angle:{angle}")
            if count % 50 == 0:
                # 定时打印 使用帮助
                print(help_str)
            count += 1

        print("退出 控制")
        noblock_term.stop_no_block()
        sys.exit(0)

def main(args=None):
    rclpy.init(args=args)
    ros_node = MinimalPublisher()
    rclpy.spin(ros_node)
    ros_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()